#include "Autonomous.h"
	
Autonomous::Autonomous(InputHandler *input, DriveTrain *drive, Camera *camera) {
	m_input = input;
	m_drive = drive;
	m_camera = camera;
	m_time = 0;
	m_endTime = 0;
	m_driveAngle = 0;
	m_backup = false;
}

void Autonomous::PureAutonomousPeriodic() {
	++m_time;
	AutoAimTurret();
	
	if (m_backup)
		//TODO: Get into backup mode
		Backup();
	else
		if (m_input->HasBalls())
			if (m_camera->hasTarget) FollowTarget();
			else SeekTarget();
		else SeekBalls();
	
	AutoFire();
}

void Autonomous::AutoAimTurret() {
	m_camera->Calculate();
	if (m_camera->hasTarget) m_camera->FollowTarget();
	else m_camera->Pan();
}

void Autonomous::SeekTarget() {
	Wander();
}

void Autonomous::SeekBalls() {
	//TODO: Really seek balls
	Wander();
}

void Autonomous::FollowTarget() {
	//TODO: Seek target - set appropriate speed
	m_drive->VDrive(m_camera->GetAngle(), 0, .5);
}

void Autonomous::AutoFire() {
	if (m_camera->CanFire()) {
		//TODO: Fire!
	}
}

void Autonomous::Wander() {
	if (!(m_time < m_endTime)) {
		m_endTime = m_time + (rand() % 150) + 51;
		m_driveAngle = rand() / (float)RAND_MAX;
	}
	m_drive->VDrive(m_driveAngle, 0, .5); //TODO: Get real speed;
}

void Autonomous::Backup() {
	//TODO: Check out if it works satisfactory
	if (!(m_time < m_endTime)) {
		m_backup = false;
	}
	m_drive->VDrive(m_driveAngle, 0, -.5); //TODO: Get real speed;
}
